package com.rgbgame.physics;

import org.jbox2d.collision.AABB;
import org.jbox2d.common.Color3f;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BoundaryListener;
import org.jbox2d.dynamics.ContactListener;
import org.jbox2d.dynamics.DebugDraw;
import org.jbox2d.dynamics.DestructionListener;
import org.jbox2d.dynamics.World;
import org.jbox2d.dynamics.joints.Joint;
import org.jbox2d.dynamics.joints.MouseJoint;
import org.jbox2d.testbed.ProcessingDebugDraw;
import org.jbox2d.testbed.TestSettings;

import com.rgbgame.RGBApplet;

public class PhysicsHandler {
	/** The PApplet that has control of the universe */
	public RGBApplet parent;
	/** A simple drawing handle */
	public DebugDraw m_debugDraw;
	/** The world object. */
    protected World m_world;
    /** Mouse joint.  May be null if mouse is not attached to anything. */
    protected MouseJoint m_mouseJoint;
    /** Settings for simulation. */
    protected TestSettings settings;
    /** The bounding box for the world. */
	protected AABB m_worldAABB;
	/** Listener for body and joint destructions. */
	protected DestructionListener m_destructionListener;
	/** Listener for world AABB violations. */
	protected BoundaryListener m_boundaryListener;
	/** Listener for contact events direct from JBox2d. */
	protected ConcreteContactListener m_contactListener;
	/** 
	 *  Dispatcher to shuffle contact events from m_contactListener to
	 *  their ultimate destinations after the step (unsafe to process
	 *  during step in case bodies must be added/deleted).  Register any
	 *  contact events here.
	 */
	protected ContactEventDispatcher m_contactEventDispatcher;
    /** Max number of contact points to store */
    static final int k_maxContactPoints = 2048; 
	/** Number of active points in m_points array. */
    protected int m_pointCount;
    /** Array of contact points - use m_pointCount to get number of active elements.  */
    protected RGBContactPoint[] m_points;
    
    public PhysicsHandler(RGBApplet _parent) {
    	parent = _parent;
    	m_debugDraw = new ProcessingDebugDraw(parent);
    	m_points = new RGBContactPoint[k_maxContactPoints];
    	m_pointCount = 0;
    	settings = new TestSettings();
    	m_destructionListener = new ConcreteDestructionListener(this);
		m_boundaryListener = new ConcreteBoundaryListener(this);
		m_contactListener = new ConcreteContactListener(this);
		m_contactEventDispatcher = new ContactEventDispatcher();
    }
    
    public void step() {
    	float timeStep = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;
		
		if (settings.pause) {
			if (settings.singleStep) {
				settings.singleStep = false;
			} else {
				timeStep = 0.0f;
			}
		}

		m_debugDraw.setFlags(0);
		if (settings.drawShapes) m_debugDraw.appendFlags(DebugDraw.e_shapeBit);
		if (settings.drawJoints) m_debugDraw.appendFlags(DebugDraw.e_jointBit);
		if (settings.drawCoreShapes) m_debugDraw.appendFlags(DebugDraw.e_coreShapeBit);
		if (settings.drawAABBs) m_debugDraw.appendFlags(DebugDraw.e_aabbBit);
		if (settings.drawOBBs) m_debugDraw.appendFlags(DebugDraw.e_obbBit);
		if (settings.drawPairs) m_debugDraw.appendFlags(DebugDraw.e_pairBit);
		if (settings.drawCOMs) m_debugDraw.appendFlags(DebugDraw.e_centerOfMassBit);

		m_world.setWarmStarting(settings.enableWarmStarting);
		m_world.setPositionCorrection(settings.enablePositionCorrection);
		m_world.setContinuousPhysics(settings.enableTOI);

		m_pointCount = 0;
		m_world.step(timeStep, settings.iterationCount);

		//Optional validation of broadphase - asserts if there is an error
		//m_world.m_broadPhase.validate();

		if (settings.drawStats) {/*
			m_debugDraw.drawString(5, m_textLine, "proxies(max) = "+m_world.m_broadPhase.m_proxyCount+
					"("+Settings.maxProxies+"), pairs(max) = "+m_world.m_broadPhase.m_pairManager.m_pairCount+
					"("+Settings.maxPairs+")", white);
			m_textLine += textLineHeight;

			m_debugDraw.drawString(5, m_textLine, "bodies/contacts/joints = "+
				m_world.m_bodyCount+"/"+m_world.m_contactCount+"/"+m_world.m_jointCount, white);
			m_textLine += textLineHeight;

			m_debugDraw.drawString(5, m_textLine, "position iterations = "+m_world.m_positionIterationCount, white);
			m_textLine += textLineHeight;

			long memTot = Runtime.getRuntime().totalMemory();
			memFree = (memFree * .9f + .1f * Runtime.getRuntime().freeMemory());
			m_debugDraw.drawString(5, m_textLine, "total memory: "+memTot, white);
			m_textLine += textLineHeight;
			m_debugDraw.drawString(5, m_textLine, "Average free memory: "+(long)memFree, white);
			m_textLine += textLineHeight;*/
		}

		if (m_mouseJoint != null) {
			Body body = m_mouseJoint.m_body2;
			Vec2 p1 = body.getWorldPoint(m_mouseJoint.m_localAnchor);
			Vec2 p2 = m_mouseJoint.m_target;

			m_debugDraw.drawSegment(p1, p2, new Color3f(255.0f,255.0f,255.0f));
		}
		

		if (settings.drawContactPoints) {
			float k_forceScale = 0.01f;
			float k_axisScale = 0.3f;

			for (int i = 0; i < m_pointCount; ++i) {
				RGBContactPoint point = m_points[i];
				
				if (point.state == 0) {
					// Add
					//System.out.println("Add");
					m_debugDraw.drawPoint(point.position, 0.3f, new Color3f(255.0f, 150.0f, 150.0f));
				} else if (point.state == 1) {
					// Persist
					//System.out.println("Persist");
					m_debugDraw.drawPoint(point.position, 0.1f, new Color3f(255.0f, 0.0f, 0.0f));
				} else {
					// Remove
					//System.out.println("Remove");
					m_debugDraw.drawPoint(point.position, 0.5f, new Color3f(0.0f, 155.0f, 155.0f));
				}

				if (settings.drawContactNormals) {
					Vec2 p1 = point.position;
					Vec2 p2 = new Vec2( p1.x + k_axisScale * point.normal.x,
										p1.y + k_axisScale * point.normal.y);
					m_debugDraw.drawSegment(p1, p2, new Color3f(0.4f*255f, 0.9f*255f, 0.4f*255f));
				} else if (settings.drawContactForces) {
					Vec2 p1 = point.position;
					Vec2 p2 = new Vec2( p1.x + k_forceScale * point.normalForce * point.normal.x,
										p1.y + k_forceScale * point.normalForce * point.normal.y);
					m_debugDraw.drawSegment(p1, p2, new Color3f(0.9f*255f, 0.9f*255f, 0.3f*255f));
				}

				if (settings.drawFrictionForces) {
					Vec2 tangent = Vec2.cross(point.normal, 1.0f);
					Vec2 p1 = point.position;
					Vec2 p2 = new Vec2( p1.x + k_forceScale * point.tangentForce * tangent.x,
										p1.y + k_forceScale * point.tangentForce * tangent.y);
					m_debugDraw.drawSegment(p1, p2, new Color3f(0.9f*255f, 0.9f*255f, 0.3f*255f));
				}
			}
		}
		
		/* 
		 * At the end of step, it is safe to react 
		 * to contact events, so emit them now.
		 */
		m_contactListener.emit(m_contactEventDispatcher);
		
    }
	
    /**
     * Create the physics world with the given boundaries
     * and gravity vector.  Note that while the gravity
     * vector may be altered after world creation, the
     * boundaries may not be.  It doesn't usually hurt performance
     * if you leave extra space when setting the bounds, so set
     * them as big as you will possibly need.
     * @param lowerBound The lower (component-wise) boundary for the world
     * @param upperBound The upper (component-wise) boundary for the world
     * @param grav The world's gravity vector
     */
    public void createWorld(Vec2 lowerBound, Vec2 upperBound, Vec2 grav) {
		m_worldAABB = new AABB();
		m_worldAABB.lowerBound = lowerBound.clone();
		m_worldAABB.upperBound = upperBound.clone();
		Vec2 gravity = grav.clone();
		boolean doSleep = true;
		m_world = new World(m_worldAABB, gravity, doSleep);
		m_world.setDestructionListener(m_destructionListener);
		m_world.setBoundaryListener(m_boundaryListener);
		m_world.setContactListener(m_contactListener);
		m_world.setDebugDraw(m_debugDraw);
	}
    
    /**
     * Returns a reference to the world object.
     * @return
     */
    public World getWorld() {
    	return m_world;
    }
    
    /**
     * Returns the world bounding box.  Changing this
     * does not change the world AABB, you must
     * recreate the world to do that.
     * @return The world AABB
     */
    public AABB getWorldAABB() {
    	return m_worldAABB;
    }
    
    /**
     * Set the world gravity to a new value.
     * @param newG
     */
    public void setGravity(Vec2 newG) {
    	m_world.setGravity(newG);
    }
    
    /** Returns a copy of world gravity vector. */
    public Vec2 getGravity() {
    	return m_world.getGravity();
    }
    
    /** Stub method for concrete examples to override if desired.
     *  Called when a joint is implicitly destroyed due to body
     *  destruction.
     *  
     *  @param joint The implicitly destroyed joint
     */
	public void jointDestroyed(Joint joint) {
		
	}

	/** Stub method for concrete examples to override if desired.
	 *  Called when a body leaves the world boundary.
	 *  
	 * @param body The body that went out of bounds
	 */
	public void boundaryViolated(Body body) {
		
	}
	
}
